#include "lane-sensor-sample-v4.h"

namespace lane_sensor_sample_v4
{
	class LaneSensorSampleVars : public SessionSampleVars
	{
	public:
		SPADAS_VARS(LaneSensorSample, SessionSampleVars)
		LaneSensorSampleData data;

		SessionGeneralSample toGeneralSample()
		{
			SessionGeneralSample sample;
			sample.timestamp() = timestamp;

			UInt segmentCount = 0;
			Array<UInt> segmentOffset(data.lines.size());
			Array<UInt> segmentSize(data.lines.size());
			for (UInt i = 0; i < data.lines.size(); i++)
			{
				segmentOffset[i] = segmentCount;
				segmentSize[i] = data.lines[i].segments.size();
				segmentCount += segmentSize[i];
			}

			Array<GeneralElement> values(23 + data.lines.size() * 27 + segmentCount * 12);
			sample.values() = values;

			values[0] = data.lines.size();
			values[1] = segmentCount;

			values[2] = data.lateralVelocity;
			values[3] = data.centerDeparture;
			values[4] = data.laneWidth;
			values[5] = data.laneHeading;
			values[6] = data.laneCurvature;
			values[7] = data.rearBound;
			values[8] = data.frontBound;
			values[9] = data.firstLeftLine.valid ? data.firstLeftLine.value : OptionalDouble();
			values[10] = data.firstRightLine.valid ? data.firstRightLine.value : OptionalDouble();
			values[11] = data.secondLeftLine.valid ? data.secondLeftLine.value : OptionalDouble();
			values[12] = data.secondRightLine.valid ? data.secondRightLine.value : OptionalDouble();
			values[13] = data.vehiVX;
			values[14] = data.vehiCurv;
			values[15] = data.vehiAX;
			values[16] = data.vehiWidth;
			values[17] = data.vehiOverhang;
			values[18] = data.vehiWheelBase;

			UInt segBase = 23 + data.lines.size() * 27;

			for (UInt i = 0; i < data.lines.size(); i++)
			{
				LaneLine& line = data.lines[i];
				UInt baseIndex = 23 + i * 27;

				values[baseIndex + 0] = (Double)line.id;
				values[baseIndex + 1] = line.rawID.valid ? (Double)line.rawID.value : OptionalDouble();
				values[baseIndex + 2] = line.confidence;
				values[baseIndex + 3] = (Double)(Int)line.description;

				if (line.modelValid())
				{
					values[baseIndex + 4] = (Double)(Int)line.clas;
					values[baseIndex + 5] = (Double)(Int)line.color;
					values[baseIndex + 6] = line.width;
					values[baseIndex + 7] = line.param[0];
					values[baseIndex + 8] = line.param[1];
					values[baseIndex + 9] = line.param[2];
					values[baseIndex + 10] = line.param[3];
					values[baseIndex + 11] = line.paramZValid ? 1.0 : 0.0;
					values[baseIndex + 12] = line.paramZ[0];
					values[baseIndex + 13] = line.paramZ[1];
					values[baseIndex + 14] = line.paramZ[2];
					values[baseIndex + 15] = line.paramZ[3];
					values[baseIndex + 16] = line.rearEnd;
					values[baseIndex + 17] = line.frontEnd;
				}

				if (line.segmentsValid())
				{
					UInt segOffset = segmentOffset[i];
					UInt segSize = segmentSize[i];
					values[baseIndex + 25] = (Double)segOffset;
					values[baseIndex + 26] = (Double)segSize;
					
					if (segSize > 0)
					{
						for (UInt n = 0; n < segSize; n++)
						{
							LineSegment& seg = line.segments[n];
							UInt segBaseIndex = segBase + 12 * (segOffset + n);

							values[segBaseIndex + 0] = (Double)seg.id;
							values[segBaseIndex + 1] = seg.p1.x;
							values[segBaseIndex + 2] = seg.p1.y;
							values[segBaseIndex + 3] = seg.z1;
							values[segBaseIndex + 4] = seg.p2.x;
							values[segBaseIndex + 5] = seg.p2.y;
							values[segBaseIndex + 6] = seg.z2;
							values[segBaseIndex + 7] = seg.solidProperty.valid ? 1.0 : 0.0;
							values[segBaseIndex + 8] = (Double)(Int)seg.solidProperty.color;
							values[segBaseIndex + 9] = seg.solidProperty.width;
						}
					}
				}

				values[baseIndex + 21] = data.getDTLC(line);
				values[baseIndex + 22] = data.getSTLC(line);
				values[baseIndex + 23] = data.getTTLC(line);
			}

			sample.significantCount() = values.size();
			return sample;
		}

		Bool fromSample(String protocol, SessionSample sample) override
		{
			if (!sample.is<SessionGeneralSample>()) return FALSE;

			auto generalSample = sample.as<SessionGeneralSample>();
			timestamp = generalSample.timestamp();
			auto values = generalSample.values();

			if (protocol == "lane-sensor-sample-v4")
			{
				if (values.size() < 23) return FALSE;
				if (!values[0].valid || !values[1].valid) return FALSE;

				Int lineCount = (Int)values[0].value;
				Int segmentCount = (Int)values[1].value;
				if ((Int)values.size() != 23 + lineCount * 27 + segmentCount * 12) return FALSE;

				data.lateralVelocity = values[2];
				data.centerDeparture = values[3];
				data.laneWidth = values[4];
				data.laneHeading = values[5];
				data.laneCurvature = values[6];
				data.rearBound = values[7].valid ? values[7].value : 0;
				data.frontBound = values[8].valid ? values[8].value : 30;
				data.firstLeftLine = values[9].valid ? (Int)values[9].value : OptionalInt();
				data.firstRightLine = values[10].valid ? (Int)values[10].value : OptionalInt();
				data.secondLeftLine = values[11].valid ? (Int)values[11].value : OptionalInt();
				data.secondRightLine = values[12].valid ? (Int)values[12].value : OptionalInt();
				data.vehiVX = values[13];
				data.vehiCurv = values[14];
				data.vehiAX = values[15];
				data.vehiWidth = values[16];
				data.vehiOverhang = values[17];
				data.vehiWheelBase = values[18];

				Int segBase = 23 + lineCount * 27;

				data.lines = Array<LaneLine>(lineCount);
				for (int i = 0; i < lineCount; i++)
				{
					LaneLine& line = data.lines[i];
					UInt baseIndex = 23 + 27 * i;

					line.id = (Int)values[baseIndex + 0].value;
					line.rawID = values[baseIndex + 1].valid ? (Int)values[baseIndex + 1].value : OptionalInt();
					line.confidence = values[baseIndex + 2];
					line.description = (LineDescription)(Int)values[baseIndex + 3].value;

					if (line.modelValid())
					{
						line.clas = (LineClass)(Int)values[baseIndex + 4].value;
						line.color = (LineColor)(Int)values[baseIndex + 5].value;
						line.width = values[baseIndex + 6];
						line.param[0] = values[baseIndex + 7].value;
						line.param[1] = values[baseIndex + 8].value;
						line.param[2] = values[baseIndex + 9].value;
						line.param[3] = values[baseIndex + 10].value;
						line.paramZValid = values[baseIndex + 11].value == 1;
						if (line.paramZValid)
						{
							line.paramZ[0] = values[baseIndex + 12].value;
							line.paramZ[1] = values[baseIndex + 13].value;
							line.paramZ[2] = values[baseIndex + 14].value;
							line.paramZ[3] = values[baseIndex + 15].value;
						}
						line.rearEnd = values[baseIndex + 16];
						line.frontEnd = values[baseIndex + 17];
					}

					if (line.segmentsValid())
					{
						Int segOffset = (Int)values[baseIndex + 25].value;
						Int segSize = (Int)values[baseIndex + 26].value;
						if (segSize > 0)
						{
							line.segments = Array<LineSegment>(segSize);
							for (Int n = 0; n < segSize; n++)
							{
								LineSegment& seg = line.segments[n];
								Int segBaseIndex = segBase + 12 * (segOffset + n);

								seg.id = (Int)values[segBaseIndex + 0].value;
								seg.p1.x = (Float)values[segBaseIndex + 1].value;
								seg.p1.y = (Float)values[segBaseIndex + 2].value;
								seg.z1 = values[segBaseIndex + 3];
								seg.p2.x = (Float)values[segBaseIndex + 4].value;
								seg.p2.y = (Float)values[segBaseIndex + 5].value;
								seg.z2 = values[segBaseIndex + 6];
								seg.solidProperty.valid = values[segBaseIndex + 7].value == 1;
								if (seg.solidProperty.valid)
								{
									seg.solidProperty.color = (LineColor)(Int)values[segBaseIndex + 8].value;
									seg.solidProperty.width = values[segBaseIndex + 9];
								}
							}
						}
					}
				}

				return TRUE;
			}
			else if (protocol == "lane-sensor-sample-v3")
			{
				if (values.size() < 17) return FALSE;
				if (!values[0].valid) return FALSE;

				int lineCount = (int)values[0].value;
				if ((Int)values.size() != 17 + lineCount * 15) return FALSE;

				data.lateralVelocity = values[1];
				data.centerDeparture = values[2];
				data.laneWidth = values[3];
				data.laneHeading = values[4];
				data.laneCurvature = values[5];
				data.vehiVX = values[6];
				data.vehiCurv = values[7];
				data.vehiAX = values[8];
				data.vehiWidth = values[9];
				data.vehiOverhang = values[10];
				data.rearBound = values[11].valid ? values[11].value : 0;
				data.frontBound = values[12].valid ? values[12].value : 30;
				data.firstLeftLine = values[13].valid ? (Int)values[13].value : OptionalInt();
				data.firstRightLine = values[14].valid ? (Int)values[14].value : OptionalInt();
				data.secondLeftLine = values[15].valid ? (Int)values[15].value : OptionalInt();
				data.secondRightLine = values[16].valid ? (Int)values[16].value : OptionalInt();

				data.lines = Array<LaneLine>(lineCount);
				for (int i = 0; i < lineCount; i++)
				{
					LaneLine& line = data.lines[i];
					UInt baseIndex = 17 + 15 * i;

					line.id = (Int)values[baseIndex + 0].value;
					line.rawID = values[baseIndex + 1].valid ? (Int)values[baseIndex + 1].value : OptionalInt();
					line.clas = (LineClass)(Int)values[baseIndex + 2].value;
					line.color = (LineColor)(Int)values[baseIndex + 3].value;
					line.confidence = values[baseIndex + 4];
					line.width = values[baseIndex + 5];
					line.param[0] = values[baseIndex + 6].value;
					line.param[1] = values[baseIndex + 7].value;
					line.param[2] = values[baseIndex + 8].value;
					line.param[3] = values[baseIndex + 9].value;
					line.rearEnd = values[baseIndex + 10];
					line.frontEnd = values[baseIndex + 11];
				}

				return TRUE;
			}
			else return FALSE;
		}

		Bool supportInterpolation() override
		{
			return TRUE;
		}

		SessionSample interpolate(SessionSample& ss1, Double w1, SessionSample& ss2, Double w2, FullTimestamp timestamp) override
		{
			if (!ss1.is<LaneSensorSample>() || !ss2.is<LaneSensorSample>()) return SessionSample();

			LaneSensorSampleData& s1 = ss1.as<LaneSensorSample>().getVars()->data;
			LaneSensorSampleData& s2 = ss2.as<LaneSensorSample>().getVars()->data;
			
			// 检查线条是否有有效ID
			Bool noID = FALSE;
			for (auto line = s1.lines.firstElem(); line.valid(); ++line)
			{
				if (line->id <= 0)
				{
					noID = TRUE;
					break;
				}
			}
			
			for (auto line = s2.lines.firstElem(); line.valid(); ++line)
			{
				if (line->id <= 0)
				{
					noID = TRUE;
					break;
				}
			}
			
			// 如果没有有效ID，选择权重更大的那个
			if (noID)
			{
				return w1 > w2 ? ss1 : ss2;
			}
			
			// 检查左右线是否匹配
			OptionalInt firstLeftID, firstRightID;
			if (s1.firstLeftLine.valid && s2.firstLeftLine.valid && 
				s1.lines[s1.firstLeftLine.value].id == s2.lines[s2.firstLeftLine.value].id)
			{
				firstLeftID = s1.lines[s1.firstLeftLine.value].id;
			}
			
			if (s1.firstRightLine.valid && s2.firstRightLine.valid && 
				s1.lines[s1.firstRightLine.value].id == s2.lines[s2.firstRightLine.value].id)
			{
				firstRightID = s1.lines[s1.firstRightLine.value].id;
			}
			
			OptionalInt secondLeftID, secondRightID;
			if (s1.secondLeftLine.valid && s2.secondLeftLine.valid && 
				s1.lines[s1.secondLeftLine.value].id == s2.lines[s2.secondLeftLine.value].id)
			{
				secondLeftID = s1.lines[s1.secondLeftLine.value].id;
			}
			
			if (s1.secondRightLine.valid && s2.secondRightLine.valid && 
				s1.lines[s1.secondRightLine.value].id == s2.lines[s2.secondRightLine.value].id)
			{
				secondRightID = s1.lines[s1.secondRightLine.value].id;
			}
			
			// 创建输出对象
			LaneSensorSample output;
			output.timestamp() = timestamp;
			LaneSensorSampleData& so = output.getVars()->data;
			
			// 插值基本数据
			so.lateralVelocity = s1.lateralVelocity * w1 + s2.lateralVelocity * w2;
			so.centerDeparture = s1.centerDeparture * w1 + s2.centerDeparture * w2;
			
			so.laneWidth = s1.laneWidth * w1 + s2.laneWidth * w2;
			so.laneCurvature = s1.laneCurvature * w1 + s2.laneCurvature * w2;
			so.laneHeading = s1.laneHeading * w1 + s2.laneHeading * w2;
			
			so.vehiVX = s1.vehiVX * w1 + s2.vehiVX * w2;
			so.vehiCurv = s1.vehiCurv * w1 + s2.vehiCurv * w2;
			so.vehiAX = s1.vehiAX * w1 + s2.vehiAX * w2;
			so.vehiWidth = s1.vehiWidth;
			so.vehiOverhang = s1.vehiOverhang;
			so.vehiWheelBase = s1.vehiWheelBase;
			
			so.rearBound = w1 > w2 ? s1.rearBound : s2.rearBound;
			so.frontBound = w1 > w2 ? s1.frontBound : s2.frontBound;
			
			// 合并车道线 - 使用ArrayX动态添加
			ArrayX<LaneLine> linesX;
			
			for (auto s1Line = s1.lines.firstElem(); s1Line.valid(); ++s1Line)
			{
				for (auto s2Line = s2.lines.firstElem(); s2Line.valid(); ++s2Line)
				{
					if (s1Line->id == s2Line->id && s1Line->description == s2Line->description)
					{
						// 创建插值线
						LaneLine line;
						line.id = s1Line->id;
						line.rawID = s1Line->rawID;
						line.confidence = s1Line->confidence * w1 + s2Line->confidence * w2;
						line.description = s1Line->description;
						
						if (line.modelValid())
						{
							line.clas = w1 > w2 ? s1Line->clas : s2Line->clas;
							line.color = w1 > w2 ? s1Line->color : s2Line->color;
							line.width = s1Line->width * w1 + s2Line->width * w2;
							line.param[0] = s1Line->param[0] * w1 + s2Line->param[0] * w2;
							line.param[1] = s1Line->param[1] * w1 + s2Line->param[1] * w2;
							line.param[2] = s1Line->param[2] * w1 + s2Line->param[2] * w2;
							line.param[3] = s1Line->param[3] * w1 + s2Line->param[3] * w2;
							
							if (s1Line->paramZValid && s2Line->paramZValid)
							{
								line.paramZValid = TRUE;
								line.paramZ[0] = s1Line->paramZ[0] * w1 + s2Line->paramZ[0] * w2;
								line.paramZ[1] = s1Line->paramZ[1] * w1 + s2Line->paramZ[1] * w2;
								line.paramZ[2] = s1Line->paramZ[2] * w1 + s2Line->paramZ[2] * w2;
								line.paramZ[3] = s1Line->paramZ[3] * w1 + s2Line->paramZ[3] * w2;
							}
							
							line.rearEnd = s1Line->rearEnd * w1 + s2Line->rearEnd * w2;
							line.frontEnd = s1Line->frontEnd * w1 + s2Line->frontEnd * w2;
						}
						
						if (line.segmentsValid())
						{
							// 使用ArrayX处理线段
							ArrayX<LineSegment> segmentsX;
							
							for (auto s1Seg = s1Line->segments.firstElem(); s1Seg.valid(); ++s1Seg)
							{
								for (auto s2Seg = s2Line->segments.firstElem(); s2Seg.valid(); ++s2Seg)
								{
									if (s1Seg->id == s2Seg->id)
									{
										LineSegment seg;
										seg.id = s1Seg->id;
										seg.p1.x = (Float)(s1Seg->p1.x * w1 + s2Seg->p1.x * w2);
										seg.p1.y = (Float)(s1Seg->p1.y * w1 + s2Seg->p1.y * w2);
										seg.z1 = s1Seg->z1 * (Float)w1 + s2Seg->z1 * (Float)w2;
										seg.p2.x = (Float)(s1Seg->p2.x * w1 + s2Seg->p2.x * w2);
										seg.p2.y = (Float)(s1Seg->p2.y * w1 + s2Seg->p2.y * w2);
										seg.z2 = s1Seg->z2 * (Float)w1 + s2Seg->z2 * (Float)w2;
										
										// 对于SolidProperty，取权重大的那个
										seg.solidProperty = w1 > w2 ? s1Seg->solidProperty : s2Seg->solidProperty;
										
										segmentsX.append(seg);
										break;
									}
								}
							}
							
							// 转换为Array
							line.segments = segmentsX.toArray();
						}
						
						// 在添加前记录索引
						Int index = linesX.size();
						if (firstLeftID.valid && firstLeftID.value == s1Line->id) so.firstLeftLine = index;
						if (firstRightID.valid && firstRightID.value == s1Line->id) so.firstRightLine = index;
						if (secondLeftID.valid && secondLeftID.value == s1Line->id) so.secondLeftLine = index;
						if (secondRightID.valid && secondRightID.value == s1Line->id) so.secondRightLine = index;
						
						linesX.append(line);
						break;
					}
				}
			}
			
			// 转换为Array
			so.lines = linesX.toArray();
			return output.as<SessionSample>();
		}
	};
}

using namespace lane_sensor_sample_v4;

LaneSensorSample::LaneSensorSample() : Object<LaneSensorSampleVars>(new LaneSensorSampleVars, TRUE)
{}

FullTimestamp& LaneSensorSample::timestamp()
{
	return vars->timestamp;
}

LaneSensorSampleData *LaneSensorSample::operator ->()
{
    return &vars->data;
}

String LaneSensorSample::getProtocolName(UInt channel)
{
    return (String)"lane-sensor-sample-v4@" + channel;
}

SessionGeneralSample LaneSensorSample::toGeneralSample()
{
    return vars->toGeneralSample();
}

Array<String> LaneSensorSample::getProtocolNames(UInt channel)
{
	return Array<String>::create(2, "lane-sensor-sample-v3", "lane-sensor-sample-v4").convert<String>([channel](auto& s){ return s + "@" + channel; });
}

Bool LaneSensorSample::fromGeneralSample(String protocol, SessionGeneralSample generalSample)
{
    return vars->fromSample(protocol, generalSample.as<SessionSample>());
}